#include "fmm.h"
#include "geodesics.h"
#include <cmath>
#include <iostream>

bool LU_solver (const MATRIX & A, const VECTOR & b, VECTOR & solution)
{

		VECTOR s (lu_solve_straight(A,b));
		VECTOR verify (A*s);
		if (std::abs (verify[1] - b[1]) > 0.1) 
		{
			return false;
		}
		else
		{
			solution = s;
			return true;
		}
		return false;		

}

//View the triangles as 
/*
        F
       / \
      A-- C
     / \ / \
    D---B---E
*/   
/*
Interpolate with the polynomial f(x ,y ) = a0 + a1 x + a2 y + a3 x^2 + a4 y ^2 + a5 xy
*/
double f(double a[], double x, double y)
{
	return (a[0] + a[1]* x + a[2]* y + a[3]* x*x + a[4]* y*y + a[5]* x*y);
}
double gradx (double coeff[], double x, double y)
{
	return( coeff[1] + 2 * coeff[3] * x + coeff[5] * y);
	
}
double grady(double coeff[], double x, double y)
{
	return ( coeff[2] + 2 * coeff[4] * y + coeff[5] * x);
}

void Huen (double coeff[], Mesh::Point & x_i, Mesh::Point & x_j, double h)
{
	Mesh::Point x_inter;
	x_inter [0] = x_i[0] - h * gradx(coeff, x_i[0], x_i[1]) ;
	x_inter [1] = x_i[1] - h * grady(coeff, x_i[0], x_i[1]) ;
	x_inter [2] = 0;

	x_j [0] = x_i[0] - h / 2.0 * (gradx(coeff, x_i[0], x_i[1]) + gradx(coeff, x_inter[0], x_inter[1]));
	x_j [1] = x_i[1] - h / 2.0 * (grady(coeff, x_i[0], x_i[1]) + grady(coeff, x_inter[0], x_inter[1]));
	x_j [2] = 0;

}
// TriA is the original triangles. TriB is the flipped one. 
// Find a rotation matrix and a translation vector to map the point from TriA to TriB
// R (A + t) = B
void convert_coordinate (Mesh::Point tri_A[3], Mesh::Point tri_B[3], MATRIX & rotation, VECTOR & translation)
{
	
	//std::cout<<"Triangle A: A "<<tri_A[0]<<std::endl;
	//std::cout<<"Triangle A: B "<<tri_A[1]<<std::endl;
	//std::cout<<"Triangle A: C "<<tri_A[2]<<std::endl;
	//std::cout<<"Triangle B: A "<<tri_B[0]<<std::endl;
	//std::cout<<"Triangle B: B "<<tri_B[1]<<std::endl;
	//std::cout<<"Triangle B: C "<<tri_B[2]<<std::endl;
	VECTOR trans(3);
	//Move the point tri_A::A to tri_B::A; 
	for (int i = 0;i < 3; ++i) trans[i] = tri_B[0][i] - tri_A[0][i]; 
	translation = trans;
	//std::cout<<"Translate"<<trans<<std::endl;

	//Translate the three points of A by the trans vector;
	for (int i = 0; i < 3; ++i)
	{
		tri_A[0][i] += trans[i]; 
		tri_A[1][i] += trans[i]; 
		tri_A[2][i] += trans[i];
	}
	//We now try to figure out the rotation matrix R, where R * (M_A ) = M_B . 
	//M_A and M_B's column vectors are the three vector's coordinates, respectively. 
	//We know that if (M_A ) is inversable, then R = M_A^{-1} * M_B
	//However M_A is not always inversable, so we use another two matrix alpha and beta to replace M_A and M_B;
	//alpha::The first two columns are the vector of B, C, the third columns are the crossproducts of BAxCA, which should be moved to A::A
	//beta::The first  two columns are the vector of B, C, the third columns are the crossproducts of BAxCA, which should be moved to A::A
	//alpha = R * beta. So R = alpha^{-1} * beta

	//Prepare the alpha.
	
	Mesh::Point alpha[3];
	alpha[0] = tri_A[1] ; alpha[1] = tri_A[2]; 
	alpha[2][0] = tri_A[1][1] * tri_A[2][2] - tri_A[2][1] * tri_A[1][2];
	alpha[2][1] = tri_A[2][0] * tri_A[1][2] - tri_A[1][0] * tri_A[2][2];
	alpha[2][2] = tri_A[1][0] * tri_A[2][1] - tri_A[2][0] * tri_A[1][1];
	//for (int i = 0; i < 3; ++i) alpha[2][i] += trans[i]; 

	
	Mesh::Point beta[3];
	beta[0] = tri_B[1] ; beta[1] = tri_B[2]; 
	beta[2][0] = tri_B[1][1] * tri_B[2][2] - tri_B[2][1] * tri_B[1][2];
	beta[2][1] = tri_B[2][0] * tri_B[1][2] - tri_B[1][0] * tri_B[2][2];
	beta[2][2] = tri_B[1][0] * tri_B[2][1] - tri_B[2][0] * tri_B[1][1];
	//for (int i = 0; i < 3; ++i) beta[2][i] += trans[i]; 
	
	
	MATRIX M_A (3,3), M_B (3,3);
	
	for (int i = 0;i < 3; ++i)
	{
		for (int j = 0; j < 3; ++j)
		{
			M_A[i][j] = alpha[j][i];
			M_B[i][j] = beta[j][i];
		}
	}

	//std::cout<<"Alpha: \n"<<M_A<<std::endl;
	//std::cout<<"Beta: \n"<<M_B<<std::endl;

	rotation =  M_B * inv(M_A);
	//std::cout<<"Rotation "<<rotation<<std::endl;
}
inline void crossproduct (const VECTOR & v1, const VECTOR & v2, VECTOR & product)
{
	VECTOR p(3);
	p[0] = v1[1] * v2[2] - v2[1] * v1[2];
	p[1] = v2[0] * v1[2] - v1[0] * v2[2];
	p[2] = v1[0] * v2[1] - v2[0] * v1[1];
	product = p;
}
inline void point2vector(const Mesh::Point & p, VECTOR & vector)
{
	VECTOR v(3);
	v[0] = p[0]; v[1] = p[1] ; v[2] = p[2];
	vector = v;
}
inline void vector2point(const VECTOR & v, Mesh::Point & p)
{
	p[0] = v[0]; p[1] = v[1]; p[2] = v[2]; 
}
inline bool is_in_triangle(const VECTOR tri[3], const VECTOR & p, int & edge_idx)
{
	VECTOR v[3];
	v[0] = tri[0] - p;
	v[1] = tri[1] - p;
	v[2] = tri[2] - p;

	VECTOR cp(3);
	for (int i = 0;i < 3; ++i)
	{
		crossproduct(v[i], v[(i+1)%3], cp);
		edge_idx = i;
		if (cp[2] < 0) return false;
	}
	return true;
}
//R (A + t) = B
void trans_A2B(VECTOR & A, VECTOR & B, MATRIX & r, VECTOR & t)
{
	VECTOR c(3);
	c = A + t;
	B = r * c ;
}
//Overload of trans points directly
void trans_A2B(Mesh::Point & A, Mesh::Point & B, MATRIX & r, VECTOR & t)
{
	VECTOR VA, VB;
	point2vector(A, VA);
	trans_A2B(VA,VB, r,t);
	vector2point(VB, B);
}
// A = R^{-1} (B - Rt)
void trans_B2A()
{

}

bool three_points_interpolation(Mesh::Point v[3], Mesh::Point & intersection, int next_face)
{

}

//Two edges, Point[0] -- Point[1]; Point[2] --- Point[3]
//Point[0] 
bool intersection(Mesh::Point point[4], Mesh::Point & p)
{
	if (fabs(point [0][1] - point [1][1]) < ERROR)
	{
		double k2 = (point[2][0] - point[3][0])/ (point[2][1] - point[3][1]);
		p[1] = point[0][1];
		p[0] = point[2][0] + k2 * (p[1] - point[2][1]);
		p[2] = 0;
		return true;
	}
	if (fabs(point[2][1] - point[3][1]) < ERROR)
	{
		double k1 = (point[0][0] - point[1][0])/ (point[0][1] - point[1][1]);
		p[1] = point[2][1];
		p[0] = point[0][0] + k1 * (p[1] - point[0][1]);
		p[2] = 0;
		return true;
	}

	double k1 = (point[0][0] - point[1][0])/ (point[0][1] - point[1][1]);
	double k2 = (point[2][0] - point[3][0])/ (point[2][1] - point[3][1]);
	double x = ((point[2][0] - k2 * point[2][1]) * (k1) - (point[0][0] - k1 * point[0][1]) * (k2)) /( k1 - k2);
	double y = ((point[2][0] - k2 * point[2][1]) - (point[0][0] - k1 * point[0][1]) ) /( k1 - k2);
	p[0] = x;
	p[1] = y;
	p[2] = 0;

	return true;
}
void print_face(Mesh & mesh, Mesh::FaceHandle fh)
{
	//Read its tree vetices
	//FaceVertexIterator Example
	Mesh::FaceVertexIter fv_it ;
	std::cout<<"f "<<std::endl;
	for ( fv_it = mesh.fv_iter(fh); fv_it; ++fv_it)
	{
		std::cout<<fv_it.handle().idx()<<' ';
		std::cout<<mesh.point(fv_it.handle())<<std::endl;
	}
	std::cout<<std::endl;
}
//This function return true if the extropolate points falls into the triangles. 
//Return false if not.
bool six_points_interpolation (Mesh::Point & start, Mesh::Point v[], double value[], double step, Mesh::Point & intsec, int & next_face)
{
	double coeff[6];
	Mesh::Point A = v[0];
	Mesh::Point B = v[1];
    Mesh::Point C = v[2];
    Mesh::Point D = v[3];
    Mesh::Point E = v[4];
    Mesh::Point F = v[5];

	double AB = (B-A).length();
	double AC = (C-A).length();
	double BC = (C-B).length();

	double AD = (D-A).length();
	double BD = (D-B).length();

	double AF = (F-A).length();
	double CF = (F-C).length();

	double CE = (E-C).length();
	double BE = (E-B).length();
	
	Mesh::Point flip_v[6];
	//Firstly, we flip the triangle ABC on the x-y plane
	flip_v[0] = Mesh::Point(0,0,0);
	flip_v[1] = Mesh::Point( AB, 0, 0);

	double theta = acos((AB* AB + AC*AC - BC*BC)/ (2.0*AB*AC));
	flip_v[2] = Mesh::Point (AC * cos(theta) , AC * sin(theta), 0);

	//Secondly, we flip the points DEF to the x-y plane
	double alpha = acos((AB* AB + AD*AD - BD*BD) / (2.0 * AB * AD));
	flip_v[3] = Mesh::Point (AD * cos(alpha), AD * sin(alpha) * (-1.0),0 );


	double mu	 = acos((BC*BC + BE * BE - CE * CE) / (2.0 * BC * BE)) ;
	double delta = acos((AB * AB + BC * BC - AC * AC) / (2.0 * AB * BC)) ;

	flip_v[4] = Mesh::Point (AB + BE * cos (PI - mu - delta), BE * sin (PI - mu - delta), 0);

	double beta = acos((AF* AF+ AC * AC - CF * CF )/ (2.0 * AF * AC));
	flip_v[5] = Mesh::Point (AF * cos(theta + beta) , AF * sin(theta + beta), 0);
	//DEBUG the flip result

	//std::cerr<<"Start "<<start<<std::endl;
	//for (int i = 0; i < 6; ++i)
	//{
	//	std::cerr<<i<<' '<<v[i]<<std::endl;
	//}
	//std::cerr<<"NA "<<flip_v[0]<<std::endl;
	//std::cerr<<"NB "<<flip_v[1]<<std::endl;
	//std::cerr<<"NC "<<flip_v[2]<<std::endl;
	//std::cerr<<"ND "<<flip_v[3]<<std::endl;
	//std::cerr<<"NE "<<flip_v[4]<<std::endl;
	//std::cerr<<"NF "<<flip_v[5]<<std::endl;

	//Interpolate the six points
	double data [6][6] ;
	//a0 + a1 x + a2 y + a3 x^2 + a4 y ^2 + a5 xy
	VECTOR b(6);
	VECTOR solution(6);
	for (int i = 0; i < 6; ++i)
	{
		data[i][0] = 1;
		data[i][1] = flip_v[i][0];
		data[i][2] = flip_v[i][1];
		data[i][3] = flip_v[i][0] * flip_v[i][0];
		data[i][4] = flip_v[i][1] * flip_v[i][1];
		data[i][5] = flip_v[i][0] * flip_v[i][1];
		b[i] = value[i];
	}
	
	MATRIX M(data);

	bool b_result = LU_solver (M,b, solution);
	if (false == b_result) return false;

	//std::cout<<"Solution: "<<solution<<std::endl;
	for (int i = 0; i < 6; ++i)
	{
		coeff[i] = solution[i];
	}
	
	//for (int i = 0; i < 6; ++i)
	//{
	//	std::cout<<f(coeff, flip_v[i][0], flip_v[i][1])<<std::endl;
	//}
	// gradient x is a1 + 2 * a3 * x + a5 * y
	// gradient y is a2 + 2 * a4 * y + a5 * x

	//std::cout<< gradx(coeff, 0.5, 0)<<' '<<grady(coeff,0.5,0)<<std::endl;
	//std::cout<< gradx(coeff, 1, 0)<<' '<<grady(coeff,1,0)<<std::endl;

	Mesh::Point tri_A[3], tri_B[3];
	for (int i = 0;i < 3; ++i)
	{
		tri_A[i] = v[i]; 
		tri_B[i] = flip_v[i];
	}
	MATRIX r;
	VECTOR t;
	convert_coordinate(tri_A, tri_B, r, t);
	//std::cerr<<"Rotation: \n"<<r<<std::endl;
	//std::cerr<<"Translation: \n"<<t<<std::endl;

	
	//for (int i = 0; i < 3; ++i)
	//{
	//	Mesh::Point B;
	//	trans_A2B(v[i], B, r, t);
	//	std::cerr<<"From "<<std::setprecision(8)<<v[i]<<std::endl;
	//	std::cerr<<"To "<<std::setprecision(8)<<B<<std::endl;
	//}

	MATRIX invr = inv(r);	
	VECTOR invt ;
	invt = -1 * r * t;


	Mesh::Point x_0 ;
	trans_A2B(start, x_0, r, t);
	//std::cerr<<"x_0 "<<x_0<<std::endl;
	Mesh::Point x_new ;
	for (int i = 0; ; ++i)
	{
		Huen(coeff, x_0, x_new, step);
		VECTOR tri_v[3], x_new_v;
		for (int i = 0; i < 3; ++i)
		{
			point2vector(flip_v[i], tri_v[i]);
		}
		point2vector(x_new, x_new_v);
		
		//Nextface 0(AB adjacent face)
		//Nextface 1(BC adjacent face)
		//Nextface 2(CA adjacent face)
		if (is_in_triangle(tri_v, x_new_v, next_face))
		{
			//std::cerr<<"In triangle"<<std::endl;
		}
		else
		{
			//std::cerr<<"Out of triangle"<<std::endl;
			Mesh::Point point[4], p;
			point [0] = flip_v[next_face];
			point [1] = flip_v[(next_face + 1 )%3];
			point [2] = x_0;
			point [3] = x_new;

			intersection(point, p);
			x_new = p;
			VECTOR B(3), A(3);
			B[0] = x_new[0]; B[1] = x_new[1]; B[2] = x_new[2]; 
			trans_A2B(B, A, invr, invt);
			Mesh::Point trans_point;
			trans_point[0] = A[0]; trans_point[1] = A[1]; trans_point[2] = A[2];
			intsec = trans_point ; 
			//std::cerr<<"v "<<std::setprecision(5)<<trans_point <<std::endl;
			break;
		}
		
		//VECTOR B(3), A(3);
		//B[0] = x_new[0]; B[1] = x_new[1]; B[2] = x_new[2]; 
		//trans_A2B(B, A, invr, invt);
		//Mesh::Point trans_point;
		//trans_point[0] = A[0]; trans_point[1] = A[1]; trans_point[2] = A[2]; 
		//std::cerr<<"v "<<std::setprecision(5)<<trans_point <<std::endl;
		
		x_0 = x_new;
	}

	return true;
}

void draw_geodesics (Mesh & mesh, const Mesh::Point & start, Mesh::FaceHandle fh, 
					 const Mesh::Point & end, const std::vector<Mesh::FaceHandle> & end_faces, 
					 double step, std::vector<Mesh::Point> & path)
{
	//std::cout<<"Start Point"<<start<<std::endl;
	path.push_back(start);
	Mesh::Point start_point = start;
	//std::cout<<"Start Face"<<std::endl;
	//print_face(mesh, fh);

//In this function, I will just whether to use six point template and three point template.
//So we need to circulate to just.
	bool flag = true;
	for (int i = 0; flag ; ++i)
	//for (int i = 0; i < 1 ; ++i) // Only draw one geodesic for debuging...
	{
		for (unsigned j = 0; j < end_faces.size(); ++j)
		{
			if (fh == end_faces[j]) 
			{
				flag = false;
				break;
			}
		}
		if (flag)
		{
			//std::cout<<"Face "<<i<<':'<<fh.idx()<<std::endl; 
			Mesh::FaceHalfedgeIter  fh_it;
			Mesh::VertexHandle vh[6]; //VertexHandles to save the six points;
			int count = 0;
			for (fh_it = mesh.fh_iter(fh); fh_it; ++fh_it, ++ count)
			{
				vh[count] = mesh.to_vertex_handle(fh_it);
				vh[count+3] = mesh.to_vertex_handle
					(mesh.next_halfedge_handle(mesh.opposite_halfedge_handle(mesh.next_halfedge_handle(fh_it))));
			}

			Mesh::Point vp[6];
			double value [6];
			for (count = 0; count < 6; ++ count)
			{
				vp[count] = mesh.point(vh[count]);
				value[count] = mesh.data(vh[count]).get_distance();
				//std::cout<<count+1<<':'<<vh[count].idx()<<' '<<vp[count]<<' '<<value[count]<<std::endl;
			}
			Mesh::Point intsec;

			int next_face;
			six_points_interpolation(start_point, vp, value, step, intsec, next_face);
			//std::cout<<"v "<<std::setprecision(5)<<intsec <<std::endl;
			path.push_back(intsec);
			start_point = intsec;
			//Find the next face
			fh_it = mesh.fh_iter(fh);
			for (int j = 0; j < next_face + 1; ++j) ++fh_it;
			fh = mesh.face_handle(mesh.opposite_halfedge_handle(fh_it));
			//Check whether the new faces is the start faces
		}
	}
	//std::cout<<"v "<<std::setprecision(5)<<end <<std::endl;
	path.push_back(end);
}

void generate_merged_mesh_geodesics(Mesh & mesh, std::vector<Mesh::Point> & path)
{
	unsigned n_vertex = mesh.n_vertices();
	
	for (unsigned i = 0; i < n_vertex; ++i)
	{
		std::cout<<"v "<<mesh.point(mesh.vertex_handle(i))<<std::endl;
	}
	
	for (unsigned i = 0; i < path.size(); ++i)
	{
		std::cout<<"v "<<path[i]<<std::endl;
	}
	
	for (Mesh::FaceIter f_it= mesh.faces_begin(); f_it!=mesh.faces_end(); ++f_it) 
	{
		std::cout<<"f ";
		for (Mesh::FaceVertexIter fv_it = mesh.fv_iter(f_it); fv_it; ++fv_it)
		{
			std::cout<<fv_it.handle().idx() + 1<<' ';
		}
		std::cout<<std::endl;
	}

	for (unsigned i = 0; i < path.size() - 1;++i)
	{	
		std::cout<<"f "<<n_vertex+ i + 1 <<' '<<n_vertex+ i + 2 << std::endl;
	}
	
}